/**
 ******************************************************************************
 * @file           : test_heading_differential_control.cpp
 * @author         : wangyingjie
 * @brief          : None
 * @attention      : None
 * @date           : 2025/5/23
 ******************************************************************************
 */

#ifdef __cplusplus
extern "C"
{
#endif

#include "control/pid/studio_pid_c.h"
#include "geography/studio_proj_c.h"
#include "geometry/studio_geo_algo_c.h"

#ifdef __cplusplus
}
#endif

// 测试使用

#include "geometry/studio_geo_utils.h"
#include "geometry/studio_geo_algo.h"

// 测试使用结束

// 小船控制结构体
typedef struct
{
    studio_pid heading_pid;  // 方位角PID控制器
    studio_point_c target;   // 目标点
    double base_throttle;    // 基础油门
    double left_throttle;    // 左侧油门
    double right_throttle;   // 右侧油门
} diff_ctrl;

// 初始化小船控制器
void init_diff_ctrl(diff_ctrl *controller,
                          const studio_point_c *target,
                          double base_throttle,  // 输入范围0~1.0
                          double Kp,
                          double Ki,
                          double Kd)
{
    init_pid(&controller->heading_pid, 0, 0, Kp, Ki, Kd, 1.0, 0.3);
    controller->target = *target;
    controller->base_throttle = base_throttle;
    controller->left_throttle = base_throttle;
    controller->right_throttle = base_throttle;
}

void reset_diff_ctrl_goal(diff_ctrl* controller, const studio_point_c* target)
{
    controller->target = *target;
    set_goal(&controller->heading_pid, 0);
}

// 正确计算最短路径误差（返回-180~180°）
double calculate_heading_error(double target_deg, double current_deg)
{
    double error = target_deg - current_deg;
    error = fmod(error + 360.0, 360.0);  // 转0~360°
    if (error > 180.0)
    {
        error -= 360.0;
    }
    return error;
}

double update_boat(diff_ctrl *controller, const studio_point_c *current_pos, double current_heading_deg)
{
    // 1. 计算目标方位角（确保函数正确）
    const double target_deg = azimuth_angle_gcs_c(current_pos, &controller->target);

    // 2. 计算最短路径误差
    double error = calculate_heading_error(target_deg, current_heading_deg);
    double steer = 0.0;

    // 3. PID控制
    if (fabs(error) > 1.0)  // 仅当误差超过1度时进行纠偏
    {
        set_goal(&controller->heading_pid, 0);
        steer = compute_pid(&controller->heading_pid, error);
    }
    else
    {

    }
    // 4. 差速控制（关键修正）
    controller->left_throttle = controller->base_throttle - steer;
    controller->right_throttle = controller->base_throttle + steer;

    // 5. 限幅保护
    controller->left_throttle = fmax(0.0, fmin(1.0, controller->left_throttle));
    controller->right_throttle = fmax(0.0, fmin(1.0, controller->right_throttle));

    return distance_squared_gcs_c(current_pos, &controller->target);
}


void simulate_movement(studio_point_c *current_pos, double *current_heading, double left_throttle, double right_throttle)
{
    const double dt = 0.1;         // 时间间隔（秒）
    const double max_speed = 1.0;  // 最大速度（米/秒）
    const double turn_rate = 0.3;  // 转向速率（度/秒 per 差速百分比）

    // 保存原始位置用于计算位移
    const studio_point_c original_pos = *current_pos;

    // 1. 计算航向变化 ---------------------------------------------
    double steer_diff = left_throttle - right_throttle;
    *current_heading += turn_rate * steer_diff * dt;
    *current_heading = fmod(*current_heading + 360.0, 360.0);

    // 2. 计算位移量（米）------------------------------------------
    double avg_throttle = (left_throttle + right_throttle) / 2.0;
    double speed = max_speed * avg_throttle;

    // 将航向角转换为弧度
    double radian = *current_heading * AO_M_PI / 180.0;

    // 计算东向和北向位移（单位：米）
    const double delta_east = speed * sin(radian) * dt;   // 东向位移（+东/-西）
    const double delta_north = speed * cos(radian) * dt;  // 北向位移（+北/-南）

    // 3. 通过投影转换更新位置 -------------------------------------
    *current_pos = lonlat_move_by(&original_pos, delta_east, delta_north);
}

int main()
{
    double central_1 = floor(120.07158158 / 3.0) * 3.0;
    double central_2 = static_cast<int>(120.07158158 / 3) * 3;
    double central_3 = floor((120.07158158 + 1.5) / 3.0) * 3.0;
    printf("===================== %s =====================\n", __FILE__);

    silly::geo::utils::init_gdal_env();
    std::string path;
#ifdef IS_LINUX
    path = "D:/9_data/2_readfile/geojson/multi_point/route_3.geojson";
#else
    path = "/home/wyj/myself/2_data/2_geojson/multi_point/route_3.geojson";
#endif
    std::cout << "path: " << path << std::endl;

    std::vector<studio_geo_coll> res_collections;

    // 差速控制测试开始 ==================
    studio_point_c beg_p = studio_point_init(120.07158158, 36.15932277);
    studio_point_c end_p = studio_point_init(120.07193922, 36.15990152);
    studio_point_c change_1_p = studio_point_init(120.07232455, 36.15964342);
    studio_point_c change_2_p = studio_point_init(120.07215543, 36.15944710);

    // 测试记录 实际删去 --------beg--------
    studio_geo_coll beg_end_points;
    beg_end_points.m_type = enum_geometry_type::egtMultiPoint;
    beg_end_points.m_points.push_back(studio_point(beg_p.x, beg_p.y));
    beg_end_points.m_points.push_back(studio_point(end_p.x, end_p.y));
    beg_end_points.m_points.push_back(studio_point(change_1_p.x, change_1_p.y));
    beg_end_points.m_points.push_back(studio_point(change_2_p.x, change_2_p.y));
    //temp_300.m_line.push_back(studio_point(beg_p.x, beg_p.y));
    //temp_100.m_line.push_back(studio_point(beg_p.x, beg_p.y));
    // 测试记录 实际删去 --------end--------

    int count = 450;  // 最大迭代次数 

    const int heads = 3;
    double current_heading[heads] = {300.0, 100.0, 27.0};
    for (int i = 0; i < heads; ++i)
    {
        studio_geo_coll temp_line;
        temp_line.m_type = enum_geometry_type::egtLineString;
        temp_line.m_points.push_back(studio_point(beg_p.x, beg_p.y));

        diff_ctrl ctrl;
        init_diff_ctrl(&ctrl, &end_p, 0.5, 0.8, 0.01, 0.2);
        studio_point_c current_pos = beg_p;
        double current_heading_deg = current_heading[i];
        for (int j = 0; j < count; ++j)
        {
            double dist_sq = update_boat(&ctrl, &current_pos, current_heading_deg);

            printf("Step %02d: 航向=%.1f° L=%.2f R=%.2f 距离²=%.1fm²\n", j + 1, current_heading_deg, ctrl.left_throttle, ctrl.right_throttle, dist_sq);

            if (dist_sq < 4.0)
            {
                printf("到达目标！\n");
                break;
            }

            simulate_movement(&current_pos, &current_heading_deg, ctrl.left_throttle, ctrl.right_throttle);
            temp_line.m_line.push_back(studio_point(current_pos.x, current_pos.y));
        }

        // 前往先一个点
        reset_diff_ctrl_goal(&ctrl, &change_1_p);
        for (int j = 0; j < count; ++j)
        {
            double dist_sq = update_boat(&ctrl, &current_pos, current_heading_deg);
            
            printf("Step %02d: 航向=%.1f° L=%.2f R=%.2f 距离²=%.1fm²\n", j + 1, current_heading_deg, ctrl.left_throttle, ctrl.right_throttle, dist_sq);

            if (dist_sq < 4.0)
            {
                printf("到达目标！\n");
                break;
            }
            simulate_movement(&current_pos, &current_heading_deg, ctrl.left_throttle, ctrl.right_throttle);
            temp_line.m_line.push_back(studio_point(current_pos.x, current_pos.y));
        }

        // 前往第二个点
        reset_diff_ctrl_goal(&ctrl, &change_2_p);
        for (int j = 0; j < count; ++j)
        {
            double dist_sq = update_boat(&ctrl, &current_pos, current_heading_deg);

            printf("Step %02d: 航向=%.1f° L=%.2f R=%.2f 距离²=%.1fm²\n", j + 1, current_heading_deg, ctrl.left_throttle, ctrl.right_throttle, dist_sq);
            if (dist_sq < 4.0)
            {
                printf("到达目标！\n");
                break;
            }
            simulate_movement(&current_pos, &current_heading_deg, ctrl.left_throttle, ctrl.right_throttle);
            temp_line.m_line.push_back(studio_point(current_pos.x, current_pos.y));
        }
        res_collections.push_back(temp_line);
    }

    res_collections.push_back(beg_end_points);
    silly::geo::utils::write_geo_coll(path, res_collections);
    silly::geo::utils::destroy_gdal_env();

    return 0;
}
